#include "hardware_driver/SerialStick.hpp"

namespace HardWareDriver
{

bool SerialStick::InitDevice(std::string dev, int baudRate)
{
    return this->serial.Open(dev, baudRate);
}

void SerialStick::ReadData()
{
    auto newMessage = serial.Read();
    for (char c : newMessage)
    {
        if (c == ':' || c == '\n')
        {
            if (currentIndex < 4)
            {
                std::string str(buff.data(), buff.size());
                buff.clear();
                int number = std::stoi(str);
                aixValues[currentIndex] = number / 500.0f;
            }
            currentIndex++;
            if (c == '\n')
            {
                currentIndex = 0;
            }
        }
        else
        {
            buff.emplace_back(c);
        }
    }
}

float SerialStick::GetAix(int aix)
{
    if (aix > 3)
        return 0;
    return aixValues[aix];
}
} // namespace HardWareDriver